
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       fms_gcs.c
  * @author     baiyang
  * @date       2021-11-10
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include <gcs_mavlink/gcs.h>

#include "fms.h"

#include <stdint.h>
#include <stdbool.h>

#include <rthw.h>
#include <rtthread.h>
#include <rtdevice.h>


#include <uITC/uITC.h>
#include <uITC/uITC_msg.h>
#include <common/time/gp_time.h>
#include <srvrly_events/srvrly_events.h>
/*-----------------------------------macro------------------------------------*/
// id(4 byte) + chan(1 byte) + sysid(1 byte) + compid(1 byte) + msg
#define FMS_GCS_MSG_HEAD_LEN (sizeof(uint32_t) + sizeof(uint8_t) + sizeof(uint8_t) + sizeof(uint8_t))

// for mavlink SET_POSITION_TARGET messages
#define MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE (POSITION_TARGET_TYPEMASK_X_IGNORE | POSITION_TARGET_TYPEMASK_Y_IGNORE | POSITION_TARGET_TYPEMASK_Z_IGNORE)
#define MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE (POSITION_TARGET_TYPEMASK_VX_IGNORE | POSITION_TARGET_TYPEMASK_VY_IGNORE | POSITION_TARGET_TYPEMASK_VZ_IGNORE)
#define MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE (POSITION_TARGET_TYPEMASK_AX_IGNORE | POSITION_TARGET_TYPEMASK_AY_IGNORE | POSITION_TARGET_TYPEMASK_AZ_IGNORE)

#define MAVLINK_SET_POS_TYPE_MASK_YAW_IGNORE POSITION_TARGET_TYPEMASK_YAW_IGNORE
#define MAVLINK_SET_POS_TYPE_MASK_YAW_RATE_IGNORE POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE
#define MAVLINK_SET_POS_TYPE_MASK_FORCE_SET POSITION_TARGET_TYPEMASK_FORCE_SET
/*----------------------------------typedef-----------------------------------*/
typedef struct {
    uint8_t chan;

    uint8_t sysid;          ///< ID of message sender system/aircraft
    uint8_t compid;         ///< ID of the message sender component

    struct rt_ringbuffer msg_rb;       // id(4 byte) + chan(1 byte) + sysid(1 byte) + compid(1 byte) + msg
    uint8_t msg_rb_pool[1024];

    mavlink_message_t mavlink_msg;

    struct rt_mutex _mutex;
} fms_gcs_msg;

/*---------------------------------prototype----------------------------------*/
static void fms_rc_manual_override(rc_channel_t c, int16_t value_in, const uint16_t offset, const float scaler, const uint32_t tnow, const bool reversed);
static void fms_handle_rc_channels_override(const mavlink_rc_channels_override_t *packet);
static void fms_handle_set_mode(const mavlink_set_mode_t* set_mode, mavlink_message_t *msg);
static void fms_push_msg(uint32_t msg_id, uint8_t chan, uint8_t sysid, uint8_t compid, uint8_t *msg_buf, uint32_t msg_len);
static uint32_t fms_get_msg_head();
static void fms_get_msg_date(uint8_t *msg_buf, uint32_t msg_len);
static void fms_proc_command_long(mavlink_command_long_t* command, mavlink_message_t* msg);
static void fms_proc_command_int(mavlink_command_int_t* command, mavlink_message_t* msg);
static MAV_RESULT fms_handle_command_component_arm_disarm(const mavlink_command_long_t *packet);
static void fms_send_command_ack(uint16_t command, uint8_t result, mavlink_message_t* msg);
static MAV_RESULT fms_handle_command_do_set_mode(const mavlink_command_long_t *packet);
static MAV_RESULT fms_set_mode_common(const MAV_MODE _base_mode, const uint32_t _custom_mode);
static void fms_gcs_cb(void *parameter);
static bool fms_should_handle_message(const mavlink_message_t* msg);
static bool fms_should_handle_command_long(const mavlink_command_long_t* command_long);
static bool fms_should_handle_command_int(mavlink_command_int_t* command_int);
static bool mavlink_coordinate_frame_to_location_alt_frame(const MAV_FRAME coordinate_frame, AltFrame *frame);
static MAV_RESULT handle_command_int_do_reposition(const mavlink_command_int_t *packet);
static bool location_from_command_t(const mavlink_command_int_t *in, Location *out);
static bool command_long_stores_location(const MAV_CMD command);
static MAV_RESULT handle_command_pause_continue(const mavlink_command_int_t *packet);
/*----------------------------------variable----------------------------------*/
static fms_gcs_msg gcs_msg;

static const float magic_force_arm_value = 2989.0f;
static const float magic_force_disarm_value = 21196.0f;
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
/**
  * @brief       
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
void fms_gcs_init()
{
    rt_ringbuffer_init(&gcs_msg.msg_rb, gcs_msg.msg_rb_pool, sizeof(gcs_msg.msg_rb_pool));

    rt_mutex_init(&gcs_msg._mutex, "fms_gcs", RT_IPC_FLAG_FIFO);

    itc_subscribe(ITC_ID(mavlink_message),  fms_gcs_cb);
    itc_subscribe(ITC_ID(mavlink_message1), fms_gcs_cb);
    itc_subscribe(ITC_ID(mavlink_message2), fms_gcs_cb);
    itc_subscribe(ITC_ID(mavlink_message3), fms_gcs_cb);
    itc_subscribe(ITC_ID(mavlink_message4), fms_gcs_cb);
    itc_subscribe(ITC_ID(mavlink_message5), fms_gcs_cb);
    itc_subscribe(ITC_ID(mavlink_message6), fms_gcs_cb);
}

/**
  * @brief       
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
void fms_gcs_update()
{
    uint32_t msg_id;
    if (rt_ringbuffer_data_len(&gcs_msg.msg_rb) < FMS_GCS_MSG_HEAD_LEN) {
        // 没有要处理的数据，直接返回
        return;
    }

    msg_id = fms_get_msg_head();

    switch (msg_id) {
        case MAVLINK_MSG_ID_COMMAND_LONG: {
            mavlink_command_long_t command;
            fms_get_msg_date((uint8_t *)&command, sizeof(mavlink_command_long_t));

            mavlink_msg_command_long_encode(gcs_msg.sysid, gcs_msg.compid, &gcs_msg.mavlink_msg, &command);
            fms_proc_command_long(&command, &gcs_msg.mavlink_msg);
        } break;

        case MAVLINK_MSG_ID_COMMAND_INT:
        {
            mavlink_command_int_t command;
            fms_get_msg_date((uint8_t *)&command, sizeof(mavlink_command_int_t));

            mavlink_msg_command_int_encode(gcs_msg.sysid, gcs_msg.compid, &gcs_msg.mavlink_msg, &command);

            fms_proc_command_int(&command, &gcs_msg.mavlink_msg);
            break;
        }

        case MAVLINK_MSG_ID_MANUAL_CONTROL:
        {
            mavlink_manual_control_t packet;
            fms_get_msg_date((uint8_t *)&packet, sizeof(mavlink_manual_control_t));

            if (packet.target != gcs_sysid_this_mav()) {
                break; // only accept control aimed at us
            }

            if (packet.z < 0) { // Copter doesn't do negative thrust
                break;
            }

            uint32_t tnow = time_millis();

            fms_rc_manual_override(fms.channel_roll, packet.y, 1000, 2000, tnow, false);
            fms_rc_manual_override(fms.channel_pitch, packet.x, 1000, 2000, tnow, true);
            fms_rc_manual_override(fms.channel_throttle, packet.z, 0, 1000, tnow, false);
            fms_rc_manual_override(fms.channel_yaw, packet.r, 1000, 2000, tnow, false);
            break;
        }
    
        case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE:
        {
            mavlink_rc_channels_override_t packet;
            fms_get_msg_date((uint8_t *)&packet, sizeof(mavlink_rc_channels_override_t));

            fms_handle_rc_channels_override(&packet);
            break;
        }

        case MAVLINK_MSG_ID_SET_MODE:
        {
            mavlink_set_mode_t packet;

            fms_get_msg_date((uint8_t *)&packet, sizeof(mavlink_set_mode_t));
            mavlink_msg_set_mode_encode(gcs_msg.sysid, gcs_msg.compid, &gcs_msg.mavlink_msg, &packet);

            fms_handle_set_mode(&packet, &gcs_msg.mavlink_msg);
            break;
        }

        case MAVLINK_MSG_ID_SET_ATTITUDE_TARGET:   // MAV ID: 82
        {
            mavlink_set_attitude_target_t packet;
            fms_get_msg_date((uint8_t *)&packet, sizeof(mavlink_set_attitude_target_t));

            // exit if vehicle is not in Guided mode or Auto-Guided mode
            if (!mode_in_guided_mode(fms.flightmode)) {
                break;
            }

            const bool roll_rate_ignore   = packet.type_mask & ATTITUDE_TARGET_TYPEMASK_BODY_ROLL_RATE_IGNORE;
            const bool pitch_rate_ignore  = packet.type_mask & ATTITUDE_TARGET_TYPEMASK_BODY_PITCH_RATE_IGNORE;
            const bool yaw_rate_ignore    = packet.type_mask & ATTITUDE_TARGET_TYPEMASK_BODY_YAW_RATE_IGNORE;
            const bool throttle_ignore    = packet.type_mask & ATTITUDE_TARGET_TYPEMASK_THROTTLE_IGNORE;
            const bool attitude_ignore    = packet.type_mask & ATTITUDE_TARGET_TYPEMASK_ATTITUDE_IGNORE;

            // ensure thrust field is not ignored
            if (throttle_ignore) {
                break;
            }

            Quat_t attitude_quat;
            if (attitude_ignore) {
                quat_zero(&attitude_quat);
            } else {
                attitude_quat.q0 = packet.q[0];
                attitude_quat.q1 = packet.q[1];
                attitude_quat.q2 = packet.q[2];
                attitude_quat.q3 = packet.q[3];

                // Do not accept the attitude_quaternion
                // if its magnitude is not close to unit length +/- 1E-3
                // this limit is somewhat greater than sqrt(FLT_EPSL)
                if (!quat_is_unit_length(&attitude_quat)) {
                    // The attitude quaternion is ill-defined
                    break;
                }
            }

            // check if the message's thrust field should be interpreted as a climb rate or as thrust
            const bool use_thrust = mguided_set_attitude_target_provides_thrust();

            float climb_rate_or_thrust;
            if (use_thrust) {
                // interpret thrust as thrust
                climb_rate_or_thrust = math_constrain_float(packet.thrust, -1.0f, 1.0f);
            } else {
                // convert thrust to climb rate
                packet.thrust = math_constrain_float(packet.thrust, 0.0f, 1.0f);
                if (math_flt_equal(packet.thrust, 0.5f)) {
                    climb_rate_or_thrust = 0.0f;
                } else if (packet.thrust > 0.5f) {
                    // climb at up to WPNAV_SPEED_UP
                    climb_rate_or_thrust = (packet.thrust - 0.5f) * 2.0f * wpnav_get_default_speed_up(fms.wp_nav);
                } else {
                    // descend at up to WPNAV_SPEED_DN
                    climb_rate_or_thrust = (0.5f - packet.thrust) * 2.0f * -wpnav_get_default_speed_down(fms.wp_nav);
                }
            }

            Vector3f_t ang_vel = VECTOR3F_DEFAULT_VALUE;
            if (!roll_rate_ignore) {
                ang_vel.x = packet.body_roll_rate;
            }
            if (!pitch_rate_ignore) {
                ang_vel.y = packet.body_pitch_rate;
            }
            if (!yaw_rate_ignore) {
                ang_vel.z = packet.body_yaw_rate;
            }

            mguided_set_angle((mode_base_t)&fms.mode_guided, &attitude_quat, &ang_vel,
                    climb_rate_or_thrust, use_thrust);

            break;
        }

        case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED:     // MAV ID: 84
        {
            // decode packet
            mavlink_set_position_target_local_ned_t packet;
            fms_get_msg_date((uint8_t *)&packet, sizeof(mavlink_set_position_target_local_ned_t));

            // exit if vehicle is not in Guided mode or Auto-Guided mode
            if (!mode_in_guided_mode(fms.flightmode)) {
                break;
            }

            // check for supported coordinate frames
            if (packet.coordinate_frame != MAV_FRAME_LOCAL_NED &&
                packet.coordinate_frame != MAV_FRAME_LOCAL_OFFSET_NED &&
                packet.coordinate_frame != MAV_FRAME_BODY_NED &&
                packet.coordinate_frame != MAV_FRAME_BODY_OFFSET_NED) {
                // input is not valid so stop
                mode_init((mode_base_t)&fms.mode_guided, true);
                break;
            }

            bool pos_ignore      = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE;
            bool vel_ignore      = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE;
            bool acc_ignore      = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE;
            bool yaw_ignore      = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_IGNORE;
            bool yaw_rate_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_RATE_IGNORE;
            bool force_set       = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_FORCE_SET;

            // Force inputs are not supported
            // Do not accept command if force_set is true and acc_ignore is false
            if (force_set && !acc_ignore) {
                break;
            }

            // prepare position
            Vector3f_t pos_vector = VECTOR3F_DEFAULT_VALUE;
            if (!pos_ignore) {
                // convert to cm
                pos_vector.x = packet.x * 100.0f;
                pos_vector.y = packet.y * 100.0f;
                pos_vector.z = -packet.z * 100.0f;

                // rotate to body-frame if necessary
                if (packet.coordinate_frame == MAV_FRAME_BODY_NED ||
                    packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED) {
                    fms_rotate_body_frame_to_NE(&pos_vector.x, &pos_vector.y);
                }
                // add body offset if necessary
                if (packet.coordinate_frame == MAV_FRAME_LOCAL_OFFSET_NED ||
                    packet.coordinate_frame == MAV_FRAME_BODY_NED ||
                    packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED) {
                    pos_vector.x += fms.ahrs->relpos_cm.x;
                    pos_vector.y += fms.ahrs->relpos_cm.y;
                    pos_vector.z += fms.ahrs->relpos_cm.z;
                }
            }

            // prepare velocity
            Vector3f_t vel_vector = VECTOR3F_DEFAULT_VALUE;
            if (!vel_ignore) {
                // convert to cm
                vel_vector.x = packet.vx * 100.0f;
                vel_vector.y = packet.vy * 100.0f;
                vel_vector.z = -packet.vz * 100.0f;

                // rotate to body-frame if necessary
                if (packet.coordinate_frame == MAV_FRAME_BODY_NED || packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED) {
                    fms_rotate_body_frame_to_NE(&vel_vector.x, &vel_vector.y);
                }
            }

            // prepare acceleration
            Vector3f_t accel_vector = VECTOR3F_DEFAULT_VALUE;
            if (!acc_ignore) {
                // convert to cm
                accel_vector.x = packet.afx * 100.0f;
                accel_vector.y = packet.afy * 100.0f;
                accel_vector.z = -packet.afz * 100.0f;

                // rotate to body-frame if necessary
                if (packet.coordinate_frame == MAV_FRAME_BODY_NED || packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED) {
                    fms_rotate_body_frame_to_NE(&accel_vector.x, &accel_vector.y);
                }
            }

            // prepare yaw
            float yaw_cd = 0.0f;
            bool yaw_relative = false;
            float yaw_rate_cds = 0.0f;
            if (!yaw_ignore) {
                yaw_cd = ToDeg(packet.yaw) * 100.0f;
                yaw_relative = packet.coordinate_frame == MAV_FRAME_BODY_NED || packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED;
            }
            if (!yaw_rate_ignore) {
                yaw_rate_cds = ToDeg(packet.yaw_rate) * 100.0f;
            }

            // send request
            if (!pos_ignore && !vel_ignore) {
                mguided_set_destination_posvelaccel((mode_base_t)&fms.mode_guided, &pos_vector, &vel_vector, &accel_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative);
            } else if (pos_ignore && !vel_ignore) {
                mguided_set_velaccel((mode_base_t)&fms.mode_guided, &vel_vector, &accel_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative, true);
            } else if (pos_ignore && vel_ignore && !acc_ignore) {
                mguided_set_accel((mode_base_t)&fms.mode_guided, &accel_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative, true);
            } else if (!pos_ignore && vel_ignore && acc_ignore) {
                mguided_set_destination((mode_base_t)&fms.mode_guided, &pos_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative, false);
            } else {
                // input is not valid so stop
                mode_init((mode_base_t)&fms.mode_guided, true);
            }
            break;
        }

        case MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT:    // MAV ID: 86
        {
            // decode packet
            mavlink_set_position_target_global_int_t packet;
            fms_get_msg_date((uint8_t *)&packet, sizeof(mavlink_set_position_target_global_int_t));

            // exit if vehicle is not in Guided mode or Auto-Guided mode
            if (!mode_in_guided_mode(fms.flightmode)) {
                break;
            }

            // todo: do we need to check for supported coordinate frames

            bool pos_ignore      = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE;
            bool vel_ignore      = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE;
            bool acc_ignore      = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE;
            bool yaw_ignore      = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_IGNORE;
            bool yaw_rate_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_RATE_IGNORE;
            bool force_set       = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_FORCE_SET;

            // Force inputs are not supported
            // Do not accept command if force_set is true and acc_ignore is false
            if (force_set && !acc_ignore) {
                break;
            }

            // extract location from message
            Location loc;
            if (!pos_ignore) {
                // sanity check location
                if (!location_check_lat_lng2(packet.lat_int, packet.lon_int)) {
                    // input is not valid so stop
                    mode_init((mode_base_t)&fms.mode_guided, true);
                    break;
                }
                AltFrame frame;
                if (!mavlink_coordinate_frame_to_location_alt_frame((MAV_FRAME)packet.coordinate_frame, &frame)) {
                    // unknown coordinate frame
                    // input is not valid so stop
                    mode_init((mode_base_t)&fms.mode_guided, true);
                    break;
                }
                location_from_lat_lon_alt(&loc, packet.lat_int, packet.lon_int, (int32_t)(packet.alt*100), frame);
            }

            // prepare velocity
            Vector3f_t vel_vector = VECTOR3F_DEFAULT_VALUE;
            if (!vel_ignore) {
                // convert to cm
                vel_vector.x = packet.vx * 100.0f;
                vel_vector.y = packet.vy * 100.0f;
                vel_vector.z = -packet.vz * 100.0f;
            }

            // prepare acceleration
            Vector3f_t accel_vector = VECTOR3F_DEFAULT_VALUE;
            if (!acc_ignore) {
                // convert to cm
                accel_vector.x = packet.afx * 100.0f;
                accel_vector.y = packet.afy * 100.0f;
                accel_vector.z = -packet.afz * 100.0f;
            }

            // prepare yaw
            float yaw_cd = 0.0f;
            float yaw_rate_cds = 0.0f;
            bool yaw_relative = false;
            if (!yaw_ignore) {
                yaw_cd = ToDeg(packet.yaw) * 100.0f;
                yaw_relative = packet.coordinate_frame == MAV_FRAME_BODY_NED || packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED;
            }
            if (!yaw_rate_ignore) {
                yaw_rate_cds = ToDeg(packet.yaw_rate) * 100.0f;
            }
    
            // send targets to the appropriate guided mode controller
            if (!pos_ignore && !vel_ignore && !acc_ignore) {
                // convert Location to vector from ekf origin for posvel controller
                if (location_get_alt_frame(&loc) == ALT_FRAME_ABOVE_TERRAIN) {
                    // posvel controller does not support alt-above-terrain
                    // input is not valid so stop
                    mode_init((mode_base_t)&fms.mode_guided, true);
                    break;
                }
                Vector3f_t pos_neu_cm;
                if (!location_get_vector_from_origin_neu(&loc, &pos_neu_cm)) {
                    // input is not valid so stop
                    mode_init((mode_base_t)&fms.mode_guided, true);
                    break;
                }
                mguided_set_destination_posvelaccel((mode_base_t)&fms.mode_guided, &pos_neu_cm, &vel_vector, &accel_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative);
            } else if (!pos_ignore && !vel_ignore) {
                // convert Location to vector from ekf origin for posvel controller
                if (location_get_alt_frame(&loc) == ALT_FRAME_ABOVE_TERRAIN) {
                    // posvel controller does not support alt-above-terrain
                    // input is not valid so stop
                    mode_init((mode_base_t)&fms.mode_guided, true);
                    break;
                }
                Vector3f_t pos_neu_cm;
                if (!location_get_vector_from_origin_neu(&loc, &pos_neu_cm)) {
                    // input is not valid so stop
                    mode_init((mode_base_t)&fms.mode_guided, true);
                    break;
                }
                mguided_set_destination_posvel((mode_base_t)&fms.mode_guided, &pos_neu_cm, &vel_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, false);
            } else if (pos_ignore && !vel_ignore) {
                mguided_set_velaccel((mode_base_t)&fms.mode_guided, &vel_vector, &accel_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, false, true);
            } else if (pos_ignore && vel_ignore && !acc_ignore) {
                mguided_set_accel((mode_base_t)&fms.mode_guided, &accel_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, false, true);
            } else if (!pos_ignore && vel_ignore && acc_ignore) {
                mguided_set_destination2((mode_base_t)&fms.mode_guided, &loc, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, false);
            } else {
                // input is not valid so stop
                mode_init((mode_base_t)&fms.mode_guided, true);
            }
            break;
        }

        case MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST: // MAV ID: 38
        case MAVLINK_MSG_ID_MISSION_ITEM:           // MAV ID: 39
        case MAVLINK_MSG_ID_MISSION_ITEM_INT:
        case MAVLINK_MSG_ID_MISSION_REQUEST_INT:
        case MAVLINK_MSG_ID_MISSION_REQUEST:
        case MAVLINK_MSG_ID_MISSION_SET_CURRENT:    // MAV ID: 41
        case MAVLINK_MSG_ID_MISSION_REQUEST_LIST:       // MAV ID: 43
        case MAVLINK_MSG_ID_MISSION_COUNT:          // MAV ID: 44
        case MAVLINK_MSG_ID_MISSION_CLEAR_ALL:      // MAV ID: 45
        case MAVLINK_MSG_ID_MISSION_ACK:
        {
            if (msg_id == MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST) {
                mavlink_mission_write_partial_list_t partial_list;
                fms_get_msg_date((uint8_t *)&partial_list, sizeof(mavlink_mission_write_partial_list_t));
                mavlink_msg_mission_write_partial_list_encode(gcs_msg.sysid, gcs_msg.compid, &gcs_msg.mavlink_msg, &partial_list);
            } else if (msg_id == MAVLINK_MSG_ID_MISSION_ITEM) {
                mavlink_mission_item_t mission;
                fms_get_msg_date((uint8_t *)&mission, sizeof(mavlink_mission_item_t));
                mavlink_msg_mission_item_encode(gcs_msg.sysid, gcs_msg.compid, &gcs_msg.mavlink_msg, &mission);
            } else if (msg_id == MAVLINK_MSG_ID_MISSION_ITEM_INT) {
                mavlink_mission_item_int_t mission_int;
                fms_get_msg_date((uint8_t *)&mission_int, sizeof(mavlink_mission_item_int_t));
                mavlink_msg_mission_item_int_encode(gcs_msg.sysid, gcs_msg.compid, &gcs_msg.mavlink_msg, &mission_int);
            } else if (msg_id == MAVLINK_MSG_ID_MISSION_REQUEST_INT) {
                mavlink_mission_request_int_t request_int;
                fms_get_msg_date((uint8_t *)&request_int, sizeof(mavlink_mission_request_int_t));
                mavlink_msg_mission_request_int_encode(gcs_msg.sysid, gcs_msg.compid, &gcs_msg.mavlink_msg, &request_int);
            } else if (msg_id == MAVLINK_MSG_ID_MISSION_REQUEST) {
                mavlink_mission_request_t request;
                fms_get_msg_date((uint8_t *)&request, sizeof(mavlink_mission_request_t));
                mavlink_msg_mission_request_encode(gcs_msg.sysid, gcs_msg.compid, &gcs_msg.mavlink_msg, &request);
            } else if (msg_id == MAVLINK_MSG_ID_MISSION_SET_CURRENT) {
                mavlink_mission_set_current_t set_current;
                fms_get_msg_date((uint8_t *)&set_current, sizeof(mavlink_mission_set_current_t));
                mavlink_msg_mission_set_current_encode(gcs_msg.sysid, gcs_msg.compid, &gcs_msg.mavlink_msg, &set_current);
            } else if (msg_id == MAVLINK_MSG_ID_MISSION_REQUEST_LIST) {
                mavlink_mission_request_list_t request_list;
                fms_get_msg_date((uint8_t *)&request_list, sizeof(mavlink_mission_request_list_t));
                mavlink_msg_mission_request_list_encode(gcs_msg.sysid, gcs_msg.compid, &gcs_msg.mavlink_msg, &request_list);
            } else if (msg_id == MAVLINK_MSG_ID_MISSION_COUNT) {
                mavlink_mission_count_t count;
                fms_get_msg_date((uint8_t *)&count, sizeof(mavlink_mission_count_t));
                mavlink_msg_mission_count_encode(gcs_msg.sysid, gcs_msg.compid, &gcs_msg.mavlink_msg, &count);
            } else if (msg_id == MAVLINK_MSG_ID_MISSION_CLEAR_ALL) {
                mavlink_mission_clear_all_t clear_all;
                fms_get_msg_date((uint8_t *)&clear_all, sizeof(mavlink_mission_clear_all_t));
                mavlink_msg_mission_clear_all_encode(gcs_msg.sysid, gcs_msg.compid, &gcs_msg.mavlink_msg, &clear_all);
            } else if (msg_id == MAVLINK_MSG_ID_MISSION_ACK) {
                mavlink_mission_ack_t ack;
                fms_get_msg_date((uint8_t *)&ack, sizeof(mavlink_mission_ack_t));
                mavlink_msg_mission_ack_encode(gcs_msg.sysid, gcs_msg.compid, &gcs_msg.mavlink_msg, &ack);
            }

            fms_handle_common_mission_message(&gcs_msg.mavlink_msg, gcs_msg.chan);
        } break;

        default: {
            gcs_send_text2(gcs_msg.chan, MAV_SEVERITY_INFO, "[fms_gcs_update]: unknown mavlink msg:%u\n", msg_id);
        } break;
    }
}

static void fms_rc_manual_override(rc_channel_t c, int16_t value_in, const uint16_t offset, const float scaler, const uint32_t tnow, const bool reversed)
{
    if (c == NULL) {
        return;
    }
    int16_t override_value = 0;
    if (value_in != INT16_MAX) {
        const int16_t radio_min = c->radio_min;
        const int16_t radio_max = c->radio_max;
        if (reversed) {
            value_in *= -1;
        }
        override_value = radio_min + (radio_max - radio_min) * (value_in + offset) / scaler;
    }
    rc_set_override(c, override_value, tnow);
}

// allow override of RC channel values for complete GCS
// control of switch position and RC PWM values.
static void fms_handle_rc_channels_override(const mavlink_rc_channels_override_t *packet)
{
    if(packet->target_system != gcs_sysid_my_gcs()) {
        return; // Only accept control from our gcs
    }

    const uint32_t tnow = time_millis();

    const uint16_t override_data[] = {
        packet->chan1_raw,
        packet->chan2_raw,
        packet->chan3_raw,
        packet->chan4_raw,
        packet->chan5_raw,
        packet->chan6_raw,
        packet->chan7_raw,
        packet->chan8_raw,
        packet->chan9_raw,
        packet->chan10_raw,
        packet->chan11_raw,
        packet->chan12_raw,
        packet->chan13_raw,
        packet->chan14_raw,
        packet->chan15_raw,
        packet->chan16_raw
    };

    for (uint8_t i=0; i<8; i++) {
        // Per MAVLink spec a value of UINT16_MAX means to ignore this field.
        if (override_data[i] != UINT16_MAX) {
            rc_set_override(rcs_chan(i), override_data[i], tnow);
        }
    }
    for (uint8_t i=8; i<ARRAY_SIZE(override_data); i++) {
        // Per MAVLink spec a value of zero or UINT16_MAX means to
        // ignore this field.
        if (override_data[i] != 0 && override_data[i] != UINT16_MAX) {
            // per the mavlink spec, a value of UINT16_MAX-1 means
            // return the field to RC radio values:
            const uint16_t value = override_data[i] == (UINT16_MAX-1) ? 0 : override_data[i];
            rc_set_override(rcs_chan(i), value, tnow);
        }
    }

    gcs_sysid_myggcs_seen(tnow);

}

/**
  * @brief       
  * @param[in]   set_mode  
  * @param[in]   msg  
  * @param[out]  
  * @retval      
  * @note        
  */
static void fms_handle_set_mode(const mavlink_set_mode_t* set_mode, mavlink_message_t *msg)
{
    const MAV_MODE _base_mode = (MAV_MODE)set_mode->base_mode;
    const uint32_t _custom_mode = set_mode->custom_mode;

    const MAV_RESULT result = fms_set_mode_common(_base_mode, _custom_mode);

    // send ACK or NAK.  Note that this is extraodinarily improper -
    // we are sending a command-ack for a message which is not a
    // command.  The command we are acking (ID=11) doesn't actually
    // exist, but if it did we'd probably be acking something
    // completely unrelated to setting modes.
    if (MAV_HAVE_PAYLOAD_SPACE(gcs_msg.chan, COMMAND_ACK)) {
        mavlink_msg_command_ack_send(gcs_msg.chan, MAVLINK_MSG_ID_SET_MODE, result,
                                     0, 0,
                                     gcs_msg.sysid,
                                     gcs_msg.compid);
    }
}

static void fms_proc_command_long(mavlink_command_long_t* command, mavlink_message_t* msg)
{
    MAV_RESULT result = MAV_RESULT_FAILED;

    switch (command->command) {
    case MAV_CMD_COMPONENT_ARM_DISARM:
        result = fms_handle_command_component_arm_disarm(command);
        break;

    case MAV_CMD_DO_SET_MODE:
        result = fms_handle_command_do_set_mode(command);
        break;

    case MAV_CMD_NAV_TAKEOFF: {
        // param3 : horizontal navigation by pilot acceptable
        // param4 : yaw angle   (not supported)
        // param5 : latitude    (not supported)
        // param6 : longitude   (not supported)
        // param7 : altitude [metres]

        float takeoff_alt = command->param7 * 100;      // Convert m to cm

        if (!mode_do_user_takeoff(fms.flightmode, takeoff_alt, math_flt_zero(command->param3))) {
            result = MAV_RESULT_FAILED;
        } else {
            result = MAV_RESULT_ACCEPTED;
        }
        break;
    }

    case MAV_CMD_DO_MOTOR_TEST:
        // param1 : motor sequence number (a number from 1 to max number of motors on the vehicle)
        // param2 : throttle type (0=throttle percentage, 1=PWM, 2=pilot throttle channel pass-through. See MOTOR_TEST_THROTTLE_TYPE enum)
        // param3 : throttle (range depends upon param2)
        // param4 : timeout (in seconds)
        // param5 : num_motors (in sequence)
        // param6 : motor test order
        result = fms_mavlink_motor_test_start(gcs_msg.chan,
                                               (uint8_t)command->param1,
                                               (uint8_t)command->param2,
                                               command->param3,
                                               command->param4,
                                               (uint8_t)command->param5);
        break;

    case MAV_CMD_DO_SET_SERVO:
        if (srvrly_do_set_servo(command->param1, command->param2)) {
            result = MAV_RESULT_ACCEPTED;
        }
        break;

    case MAV_CMD_DO_REPEAT_SERVO:
        if (srvrly_do_repeat_servo(command->param1, command->param2, command->param3, command->param4 * 1000)) {
            result = MAV_RESULT_ACCEPTED;
        }
        break;

#if MB_RELAY_ENABLED
    case MAV_CMD_DO_SET_RELAY:
        if (srvrly_do_set_relay(command->param1, command->param2)) {
            result = MAV_RESULT_ACCEPTED;
        }
        break;

    case MAV_CMD_DO_REPEAT_RELAY:
        if (srvrly_do_repeat_relay(command->param1, command->param2, command->param3 * 1000)) {
            result = MAV_RESULT_ACCEPTED;
        }
        break;
#endif

    default:
        break;
    }

    fms_send_command_ack(command->command, result, msg);
}

static void fms_proc_command_int(mavlink_command_int_t* command, mavlink_message_t* msg)
{
    MAV_RESULT result = MAV_RESULT_FAILED;

    switch (command->command) {
    case MAV_CMD_DO_REPOSITION:
        result = handle_command_int_do_reposition(command);
        break;

    case MAV_CMD_DO_PAUSE_CONTINUE:
        result = handle_command_pause_continue(command);
        break;

    default:
        break;
    }

    fms_send_command_ack(command->command, result, msg);
}

static void fms_send_command_ack(uint16_t command, uint8_t result, mavlink_message_t* msg)
{
    if (MAV_HAVE_PAYLOAD_SPACE(gcs_msg.chan, COMMAND_ACK)) {
        mavlink_msg_command_ack_send(gcs_msg.chan, command, result,
                                     0, 0,
                                     msg->sysid,
                                     msg->compid);
    }
}

static MAV_RESULT fms_handle_command_component_arm_disarm(const mavlink_command_long_t *packet)
{
    if (math_flt_equal(packet->param1,1.0f)) {
        if (mb_arming_is_armed()) {
            return MAV_RESULT_ACCEPTED;
        }
        // run pre_arm_checks and arm_checks and display failures
        const bool do_arming_checks = !math_flt_equal(packet->param2,magic_force_arm_value);
        if (mb_arming_arm(ARMING_METHOD_MAVLINK, do_arming_checks)) {
            return MAV_RESULT_ACCEPTED;
        }
        return MAV_RESULT_FAILED;
    }
    if (math_flt_zero(packet->param1))  {
        if (!mb_arming_is_armed()) {
            return MAV_RESULT_ACCEPTED;
        }
        const bool forced = math_flt_equal(packet->param2, magic_force_disarm_value);
        // note disarm()'s second parameter is "do_disarm_checks"
        if (mb_arming_disarm(ARMING_METHOD_MAVLINK, !forced)) {
            return MAV_RESULT_ACCEPTED;
        }
        return MAV_RESULT_FAILED;
    }

    return MAV_RESULT_UNSUPPORTED;
}

static MAV_RESULT fms_handle_command_do_set_mode(const mavlink_command_long_t *packet)
{
    const MAV_MODE _base_mode = (MAV_MODE)packet->param1;
    const uint32_t _custom_mode = (uint32_t)packet->param2;

    return fms_set_mode_common(_base_mode, _custom_mode);
}

/*
  code common to both SET_MODE mavlink message and command long set_mode msg
*/
static MAV_RESULT fms_set_mode_common(const MAV_MODE _base_mode, const uint32_t _custom_mode)
{
    // only accept custom modes because there is no easy mapping from Mavlink flight modes to AC flight modes
    if ((uint32_t)_base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) {
        if (!fms_set_mode(_custom_mode, MODE_REASON_GCS_COMMAND)) {
            // often we should be returning DENIED rather than FAILED
            // here.  Perhaps a "has_mode" callback on AP_::vehicle()
            // would do?
            return MAV_RESULT_FAILED;
        }
        return MAV_RESULT_ACCEPTED;
    }

    if (_base_mode == (MAV_MODE)MAV_MODE_FLAG_DECODE_POSITION_SAFETY) {
        // set the safety switch position. Must be in a command by itself
        if (_custom_mode == 0) {
            // turn safety off (pwm outputs flow to the motors)
            //hal.rcout->force_safety_off();
            return MAV_RESULT_ACCEPTED;
        }
        if (_custom_mode == 1) {
#if 0
            // turn safety on (no pwm outputs to the motors)
            if (hal.rcout->force_safety_on()) {
                return MAV_RESULT_ACCEPTED;
            }
            return MAV_RESULT_FAILED;
#endif
            return MAV_RESULT_ACCEPTED;
        }
        return MAV_RESULT_DENIED;
    }

    // Command is invalid (is supported but has invalid parameters)
    return MAV_RESULT_DENIED;
}

/**
  * @brief       
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
static void fms_gcs_cb(void *parameter)
{
    uitc_mavlink_message *mavlink_msg = (uitc_mavlink_message *)parameter;

    if (!fms_should_handle_message(mavlink_msg->msg_t)) {
        return;
    }

    switch (mavlink_msg->msg_t->msgid) {
        case MAVLINK_MSG_ID_COMMAND_LONG: {
            mavlink_command_long_t command;
            mavlink_msg_command_long_decode(mavlink_msg->msg_t, &command);

            fms_push_msg(MAVLINK_MSG_ID_COMMAND_LONG, mavlink_msg->chan, mavlink_msg->msg_t->sysid, mavlink_msg->msg_t->compid, (uint8_t *)&command, sizeof(mavlink_command_long_t));
        } break;

        case MAVLINK_MSG_ID_COMMAND_INT:
        {
            mavlink_command_int_t command;
            mavlink_msg_command_int_decode(mavlink_msg->msg_t, &command);

            fms_push_msg(MAVLINK_MSG_ID_COMMAND_INT, mavlink_msg->chan, mavlink_msg->msg_t->sysid, mavlink_msg->msg_t->compid, (uint8_t *)&command, sizeof(mavlink_command_int_t));
            break;
        }

        case MAVLINK_MSG_ID_MANUAL_CONTROL:
        {
            mavlink_manual_control_t packet;
            mavlink_msg_manual_control_decode(mavlink_msg->msg_t, &packet);

            fms_push_msg(MAVLINK_MSG_ID_MANUAL_CONTROL, mavlink_msg->chan, mavlink_msg->msg_t->sysid, mavlink_msg->msg_t->compid, (uint8_t *)&packet, sizeof(mavlink_manual_control_t));
            break;
        }
    
        case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE:
        {
            mavlink_rc_channels_override_t packet;
            mavlink_msg_rc_channels_override_decode(mavlink_msg->msg_t, &packet);

            fms_push_msg(MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE, mavlink_msg->chan, mavlink_msg->msg_t->sysid, mavlink_msg->msg_t->compid, (uint8_t *)&packet, sizeof(mavlink_rc_channels_override_t));
            break;
        }

        case MAVLINK_MSG_ID_SET_MODE:
        {
            mavlink_set_mode_t packet;
            mavlink_msg_set_mode_decode(mavlink_msg->msg_t, &packet);

            fms_push_msg(MAVLINK_MSG_ID_SET_MODE, mavlink_msg->chan, mavlink_msg->msg_t->sysid, mavlink_msg->msg_t->compid, (uint8_t *)&packet, sizeof(mavlink_set_mode_t));
            break;
        }

        case MAVLINK_MSG_ID_SET_ATTITUDE_TARGET:   // MAV ID: 82
        {
            mavlink_set_attitude_target_t packet;
            mavlink_msg_set_attitude_target_decode(mavlink_msg->msg_t, &packet);

            fms_push_msg(MAVLINK_MSG_ID_SET_ATTITUDE_TARGET, mavlink_msg->chan, mavlink_msg->msg_t->sysid, mavlink_msg->msg_t->compid, (uint8_t *)&packet, sizeof(mavlink_set_attitude_target_t));
            break;
        }

        case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED:     // MAV ID: 84
        {
            // decode packet
            mavlink_set_position_target_local_ned_t packet;
            mavlink_msg_set_position_target_local_ned_decode(mavlink_msg->msg_t, &packet);

            fms_push_msg(MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED, mavlink_msg->chan, mavlink_msg->msg_t->sysid, mavlink_msg->msg_t->compid, (uint8_t *)&packet, sizeof(mavlink_set_position_target_local_ned_t));
            break;
        }

        case MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT:    // MAV ID: 86
        {
            // decode packet
            mavlink_set_position_target_global_int_t packet;
            mavlink_msg_set_position_target_global_int_decode(mavlink_msg->msg_t, &packet);

            fms_push_msg(MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT, mavlink_msg->chan, mavlink_msg->msg_t->sysid, mavlink_msg->msg_t->compid, (uint8_t *)&packet, sizeof(mavlink_set_position_target_global_int_t));
            break;
        }

        case MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST: // MAV ID: 38
        {
            // decode packet
            mavlink_mission_write_partial_list_t packet;
            mavlink_msg_mission_write_partial_list_decode(mavlink_msg->msg_t, &packet);

            fms_push_msg(MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST, mavlink_msg->chan, mavlink_msg->msg_t->sysid, mavlink_msg->msg_t->compid, (uint8_t *)&packet, sizeof(mavlink_mission_write_partial_list_t));
            break;
        }

        case MAVLINK_MSG_ID_MISSION_ITEM:    // MAV ID: 39
        {
            // decode packet
            mavlink_mission_item_t packet;
            mavlink_msg_mission_item_decode(mavlink_msg->msg_t, &packet);

            fms_push_msg(MAVLINK_MSG_ID_MISSION_ITEM, mavlink_msg->chan, mavlink_msg->msg_t->sysid, mavlink_msg->msg_t->compid, (uint8_t *)&packet, sizeof(mavlink_mission_item_t));
            break;
        }

        case MAVLINK_MSG_ID_MISSION_ITEM_INT:    // MAV ID: 73
        {
            // decode packet
            mavlink_mission_item_int_t packet;
            mavlink_msg_mission_item_int_decode(mavlink_msg->msg_t, &packet);

            fms_push_msg(MAVLINK_MSG_ID_MISSION_ITEM_INT, mavlink_msg->chan, mavlink_msg->msg_t->sysid, mavlink_msg->msg_t->compid, (uint8_t *)&packet, sizeof(mavlink_mission_item_int_t));
            break;
        }

        case MAVLINK_MSG_ID_MISSION_REQUEST_INT:
        {
            // decode packet
            mavlink_mission_request_int_t packet;
            mavlink_msg_mission_request_int_decode(mavlink_msg->msg_t, &packet);

            fms_push_msg(MAVLINK_MSG_ID_MISSION_REQUEST_INT, mavlink_msg->chan, mavlink_msg->msg_t->sysid, mavlink_msg->msg_t->compid, (uint8_t *)&packet, sizeof(mavlink_mission_request_int_t));
            break;
        }

        case MAVLINK_MSG_ID_MISSION_REQUEST:
        {
            // decode packet
            mavlink_mission_request_t packet;
            mavlink_msg_mission_request_decode(mavlink_msg->msg_t, &packet);

            fms_push_msg(MAVLINK_MSG_ID_MISSION_REQUEST, mavlink_msg->chan, mavlink_msg->msg_t->sysid, mavlink_msg->msg_t->compid, (uint8_t *)&packet, sizeof(mavlink_mission_request_t));
            break;
        }

        case MAVLINK_MSG_ID_MISSION_SET_CURRENT:    // MAV ID: 41
        {
            // decode packet
            mavlink_mission_set_current_t packet;
            mavlink_msg_mission_set_current_decode(mavlink_msg->msg_t, &packet);

            fms_push_msg(MAVLINK_MSG_ID_MISSION_SET_CURRENT, mavlink_msg->chan, mavlink_msg->msg_t->sysid, mavlink_msg->msg_t->compid, (uint8_t *)&packet, sizeof(mavlink_mission_set_current_t));
            break;
        }

        case MAVLINK_MSG_ID_MISSION_REQUEST_LIST:       // MAV ID: 43
        {
            // decode packet
            mavlink_mission_request_list_t packet;
            mavlink_msg_mission_request_list_decode(mavlink_msg->msg_t, &packet);

            fms_push_msg(MAVLINK_MSG_ID_MISSION_REQUEST_LIST, mavlink_msg->chan, mavlink_msg->msg_t->sysid, mavlink_msg->msg_t->compid, (uint8_t *)&packet, sizeof(mavlink_mission_request_list_t));
            break;
        }

        case MAVLINK_MSG_ID_MISSION_COUNT:          // MAV ID: 44
        {
            // decode packet
            mavlink_mission_count_t packet;
            mavlink_msg_mission_count_decode(mavlink_msg->msg_t, &packet);

            fms_push_msg(MAVLINK_MSG_ID_MISSION_COUNT, mavlink_msg->chan, mavlink_msg->msg_t->sysid, mavlink_msg->msg_t->compid, (uint8_t *)&packet, sizeof(mavlink_mission_count_t));
            break;
        }

        case MAVLINK_MSG_ID_MISSION_CLEAR_ALL:      // MAV ID: 45
        {
            // decode packet
            mavlink_mission_clear_all_t packet;
            mavlink_msg_mission_clear_all_decode(mavlink_msg->msg_t, &packet);

            fms_push_msg(MAVLINK_MSG_ID_MISSION_CLEAR_ALL, mavlink_msg->chan, mavlink_msg->msg_t->sysid, mavlink_msg->msg_t->compid, (uint8_t *)&packet, sizeof(mavlink_mission_clear_all_t));
            break;
        }

        case MAVLINK_MSG_ID_MISSION_ACK:
        {
            // decode packet
            mavlink_mission_ack_t packet;
            mavlink_msg_mission_ack_decode(mavlink_msg->msg_t, &packet);

            fms_push_msg(MAVLINK_MSG_ID_MISSION_ACK, mavlink_msg->chan, mavlink_msg->msg_t->sysid, mavlink_msg->msg_t->compid, (uint8_t *)&packet, sizeof(mavlink_mission_ack_t));
            break;
        }

        default: {
            gcs_send_text2(mavlink_msg->chan, MAV_SEVERITY_INFO, "[fms_gcs_cb]: unknown mavlink msg:%u\n", mavlink_msg->msg_t->msgid);
        } break;
    }
}

/**
  * @brief       
  * @param[in]   msg_id  
  * @param[in]   msg_buf  
  * @param[in]   msg_len  
  * @param[out]  
  * @retval      
  * @note        
  */
static void fms_push_msg(uint32_t msg_id, uint8_t chan, uint8_t sysid, uint8_t compid, uint8_t *msg_buf, uint32_t msg_len)
{
    if (rt_ringbuffer_space_len(&gcs_msg.msg_rb) >= msg_len + FMS_GCS_MSG_HEAD_LEN) {

        rt_mutex_take(&gcs_msg._mutex, RT_WAITING_FOREVER);

        rt_ringbuffer_put(&gcs_msg.msg_rb, (uint8_t *)&msg_id, sizeof(msg_id));
        rt_ringbuffer_putchar(&gcs_msg.msg_rb, chan);
        rt_ringbuffer_putchar(&gcs_msg.msg_rb, sysid);
        rt_ringbuffer_putchar(&gcs_msg.msg_rb, compid);
        rt_ringbuffer_put(&gcs_msg.msg_rb, msg_buf, msg_len);

        rt_mutex_release(&gcs_msg._mutex);
    } else {
        rt_mutex_take(&gcs_msg._mutex, RT_WAITING_FOREVER);
        rt_ringbuffer_reset(&gcs_msg.msg_rb);

        rt_ringbuffer_put(&gcs_msg.msg_rb, (uint8_t *)&msg_id, sizeof(msg_id));
        rt_ringbuffer_putchar(&gcs_msg.msg_rb, chan);
        rt_ringbuffer_putchar(&gcs_msg.msg_rb, sysid);
        rt_ringbuffer_putchar(&gcs_msg.msg_rb, compid);
        rt_ringbuffer_put(&gcs_msg.msg_rb, msg_buf, msg_len);

        rt_mutex_release(&gcs_msg._mutex);

        gcs_send_text2(chan, MAV_SEVERITY_INFO, "fms_gcs miss msg");
    }
}

/**
  * @brief       
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
static uint32_t fms_get_msg_head()
{
    uint32_t msg_id;
    rt_mutex_take(&gcs_msg._mutex, RT_WAITING_FOREVER);
    rt_ringbuffer_get(&gcs_msg.msg_rb, (uint8_t *)&msg_id, sizeof(msg_id));
    rt_ringbuffer_getchar(&gcs_msg.msg_rb, &gcs_msg.chan);
    rt_ringbuffer_getchar(&gcs_msg.msg_rb, &gcs_msg.sysid);
    rt_ringbuffer_getchar(&gcs_msg.msg_rb, &gcs_msg.compid);
    rt_mutex_release(&gcs_msg._mutex);

    return msg_id;
}

/**
  * @brief       
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
static void fms_get_msg_date(uint8_t *msg_buf, uint32_t msg_len)
{
    rt_mutex_take(&gcs_msg._mutex, RT_WAITING_FOREVER);
    rt_ringbuffer_get(&gcs_msg.msg_rb, msg_buf, msg_len);
    rt_mutex_release(&gcs_msg._mutex);
}

static bool fms_should_handle_message(const mavlink_message_t* msg)
{
    bool res = false;

    switch (msg->msgid) {
        case MAVLINK_MSG_ID_COMMAND_LONG:
        {
            mavlink_command_long_t command;
            mavlink_msg_command_long_decode(msg, &command);

            res = fms_should_handle_command_long(&command);
            break;
        }
        case MAVLINK_MSG_ID_COMMAND_INT:
        {
            mavlink_command_int_t command;
            mavlink_msg_command_int_decode(msg, &command);

            res = fms_should_handle_command_int(&command);
            break;
        }

        case MAVLINK_MSG_ID_MANUAL_CONTROL:
        case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE:
        case MAVLINK_MSG_ID_SET_MODE:
        case MAVLINK_MSG_ID_SET_ATTITUDE_TARGET:
        case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED:
        case MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT:
        case MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST:
        case MAVLINK_MSG_ID_MISSION_ITEM:
        case MAVLINK_MSG_ID_MISSION_ITEM_INT:
        case MAVLINK_MSG_ID_MISSION_REQUEST_INT:
        case MAVLINK_MSG_ID_MISSION_REQUEST:
        case MAVLINK_MSG_ID_MISSION_SET_CURRENT:
        case MAVLINK_MSG_ID_MISSION_REQUEST_LIST:
        case MAVLINK_MSG_ID_MISSION_COUNT:
        case MAVLINK_MSG_ID_MISSION_CLEAR_ALL:
        case MAVLINK_MSG_ID_MISSION_ACK:
            res = true;
            break;
        default:
            break;
    }

    return res;
}

static bool fms_should_handle_command_long(const mavlink_command_long_t* command_long)
{
    bool res = false;

    switch (command_long->command) {
    case MAV_CMD_COMPONENT_ARM_DISARM:
    case MAV_CMD_DO_SET_MODE:
    case MAV_CMD_NAV_TAKEOFF:
    case MAV_CMD_DO_MOTOR_TEST:
    case MAV_CMD_DO_SET_SERVO:
    case MAV_CMD_DO_SET_RELAY:
    case MAV_CMD_DO_REPEAT_SERVO:
    case MAV_CMD_DO_REPEAT_RELAY:
        res = true;
        break;
    default:
        break;
    }

    return res;
}

static bool fms_should_handle_command_int(mavlink_command_int_t* command_int)
{
    bool res = false;

    switch (command_int->command) {
    case MAV_CMD_DO_REPOSITION:
    case MAV_CMD_DO_PAUSE_CONTINUE:
        res = true;
        break;
    default:
        break;
    }

    return res;
}

static bool mavlink_coordinate_frame_to_location_alt_frame(const MAV_FRAME coordinate_frame, AltFrame *frame)
{
    switch (coordinate_frame) {
    case MAV_FRAME_GLOBAL_RELATIVE_ALT: // solo shot manager incorrectly sends RELATIVE_ALT instead of RELATIVE_ALT_INT
    case MAV_FRAME_GLOBAL_RELATIVE_ALT_INT:
        *frame = ALT_FRAME_ABOVE_HOME;
        return true;
    case MAV_FRAME_GLOBAL_TERRAIN_ALT:
    case MAV_FRAME_GLOBAL_TERRAIN_ALT_INT:
        *frame = ALT_FRAME_ABOVE_TERRAIN;
        return true;
    case MAV_FRAME_GLOBAL:
    case MAV_FRAME_GLOBAL_INT:
        *frame = ALT_FRAME_ABSOLUTE;
        return true;
    default:
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL_WIN
        gcs_send_text2(gcs_msg.chan, MAV_SEVERITY_INFO, "Unknown mavlink coordinate frame %u", coordinate_frame);
#endif
        return false;
    }
}

static MAV_RESULT handle_command_int_do_reposition(const mavlink_command_int_t *packet)
{
    const bool change_modes = ((int32_t)packet->param2 & MAV_DO_REPOSITION_FLAGS_CHANGE_MODE) == MAV_DO_REPOSITION_FLAGS_CHANGE_MODE;
    if (!mode_in_guided_mode(fms.flightmode) && !change_modes) {
        return MAV_RESULT_DENIED;
    }

    // sanity check location
    if (!location_check_lat_lng2(packet->x, packet->y)) {
        return MAV_RESULT_DENIED;
    }

    Location request_location;
    if (!location_from_command_t(packet, &request_location)) {
        return MAV_RESULT_DENIED;
    }

    if (location_sanitize(&request_location, &fms.current_loc)) {
        // if the location wasn't already sane don't load it
        return MAV_RESULT_DENIED; // failed as the location is not valid
    }

    // we need to do this first, as we don't want to change the flight mode unless we can also set the target
    if (!mguided_set_destination2((mode_base_t)&fms.mode_guided, &request_location, false, 0, false, 0, false)) {
        return MAV_RESULT_FAILED;
    }

    if (!mode_in_guided_mode(fms.flightmode)) {
        if (!fms_set_mode(GUIDED, MODE_REASON_GCS_COMMAND)) {
            return MAV_RESULT_FAILED;
        }
        // the position won't have been loaded if we had to change the flight mode, so load it again
        if (!mguided_set_destination2((mode_base_t)&fms.mode_guided, &request_location, false, 0, false, 0, false)) {
            return MAV_RESULT_FAILED;
        }
    }

    return MAV_RESULT_ACCEPTED;
}

static bool location_from_command_t(const mavlink_command_int_t *in, Location *out)
{
    if (!command_long_stores_location((MAV_CMD)in->command)) {
        return false;
    }

    // integer storage imposes limits on the altitudes we can accept:
    if (fabsf(in->z) > LOCATION_ALT_MAX_M) {
        return false;
    }

    AltFrame frame;
    if (!mavlink_coordinate_frame_to_location_alt_frame((MAV_FRAME)in->frame, &frame)) {
        // unknown coordinate frame
        return false;
    }

    out->lat = in->x;
    out->lng = in->y;

    location_set_alt_cm(out, (int32_t)(in->z * 100), frame);

    return true;
}

static bool command_long_stores_location(const MAV_CMD command)
{
    switch(command) {
    case MAV_CMD_DO_SET_HOME:
    case MAV_CMD_DO_SET_ROI:
    case MAV_CMD_DO_SET_ROI_LOCATION:
    case MAV_CMD_NAV_TAKEOFF:
    case MAV_CMD_DO_REPOSITION:
        return true;
    default:
        return false;
    }
    return false;
}

static MAV_RESULT handle_command_pause_continue(const mavlink_command_int_t *packet)
{
    // requested pause
    if ((uint8_t) packet->param1 == 0) {
        if (mode_pause((mode_base_t)fms.flightmode)) {
            return MAV_RESULT_ACCEPTED;
        }
        gcs_send_text2(gcs_msg.chan, MAV_SEVERITY_INFO, "Failed to pause");
        return MAV_RESULT_FAILED;
    }

    // requested resume
    if ((uint8_t) packet->param1 == 1) {
        if (mode_resume((mode_base_t)fms.flightmode)) {
            return MAV_RESULT_ACCEPTED;
        }
        gcs_send_text2(gcs_msg.chan, MAV_SEVERITY_INFO, "Failed to resume");
        return MAV_RESULT_FAILED;
    }
    return MAV_RESULT_DENIED;
}
/*------------------------------------test------------------------------------*/


